#include "irobot_abstract2.h"
#include "oi.h"
#include <stdio.h>
#include <math.h>


unsigned char sensor_data[52];

void collect_sensor_data () {
	ir_sleep(10);
	ir_send_byte (CmdSensors);
	ir_send_byte (6);

	int i;
	for (i = 0; i < 52; i++)
		sensor_data[i] = ir_get_byte();

	return;
}

bool left_bumper () {
	return sensor_data[0] & (1 << 1);
}
bool right_bumper () {
	return sensor_data[0] & 1;
}

bool play_button (){
	return sensor_data[11] & 1;
}
bool advance_button (){
	return sensor_data[11] & (1 << 3);
}

unsigned short infrared () {
	return (unsigned short) sensor_data[10];
}

unsigned char charging_sources(){
	return ((unsigned char)sensor_data[39]);
}

signed short current(){
	return ((signed short)sensor_data[19] << 8) | ((signed short)sensor_data[20]);
}

unsigned short wall_signal () {
	printf("wall_raw: %x %x\n",sensor_data[26],sensor_data[27]);
	return ((unsigned short)sensor_data[26] << 8) | ((unsigned short)sensor_data[27]);
}

short distance() {
	return ((short)sensor_data[12] << 8) | ((short)sensor_data[13]);
}

short angle() {
	return ((short)sensor_data[14] << 8) | ((short)sensor_data[15]);
}

bool cliff_left(void) {
	return (bool) sensor_data[2];
}
bool cliff_right(void) {
	return (bool) sensor_data[5];
}
bool cliff_front_left(void) {
	return (bool) sensor_data[3];
}
bool cliff_front_right(void) {
	return (bool) sensor_data[4];
}
unsigned short cliff_left_signal(void){
	return ((unsigned short)sensor_data[28] << 8) | ((unsigned short)sensor_data[29]);
}
unsigned short cliff_right_signal(void){
	return ((unsigned short)sensor_data[34] << 8) | ((unsigned short)sensor_data[35]);
}
unsigned short cliff_front_left_signal(void){
	return ((unsigned short)sensor_data[30] << 8) | ((unsigned short)sensor_data[31]);
}
unsigned short cliff_front_right_signal(void){
	return ((unsigned short)sensor_data[32] << 8) | ((unsigned short)sensor_data[33]);
}


//O'Kane says wheel base is 258mm
const unsigned int WHEEL_BASE = 258;

//get the most significant byte from a short
unsigned char get_low_byte(short two_bytes){
	unsigned char a = (two_bytes & 0x000000ff);	

	return a;

}

//get the least significant byte from a short
unsigned char get_high_byte(short two_bytes){
  	unsigned char a = (two_bytes & 0x0000ff00) >> 8;

	return a;
}

void stop(){
	ir_send_byte(CmdDriveWheels);
	ir_send_byte(0);
	ir_send_byte(0);
	ir_send_byte(0);
	ir_send_byte(0);
}

//drive with the specified wheel velocities
void drive(short right_velocity, short left_velocity){
	ir_send_byte(CmdDriveWheels);
	ir_send_byte(get_high_byte(right_velocity));
	ir_send_byte(get_low_byte(right_velocity));
	ir_send_byte(get_high_byte(left_velocity));
	ir_send_byte(get_low_byte(left_velocity));
}

//returns the number of milliseconds it will take to go
//the specified distance at the given velocity
unsigned int time_to_drive_distance(short velocity, int centimeters){
	//convert distance to mm
	unsigned int distance = centimeters * 10;

	//convert time directly to ms to ensure no loss of precision
	unsigned int time = (distance * 1000) / velocity;
	
	return time;
}

/*
//drive the specified number of milliseconds at the specified velocitites
void drive_duration(short right_velocity, short left_velocity, unsigned int milliseconds){
	drive(right_velocity, left_velocity);	

	ir_sleep(milliseconds);

	stop();
}

//drive straight forward or backward with the specified velocity
void drive_direct(short velocity){
	drive(velocity, velocity);
}

//drive in a straight line for the specified distance at the specified speed
void drive_direct_distance(short velocity, int centimeters){
	//convert distance to mm
	unsigned int distance = centimeters * 10;

	//convert time directly to ms to ensure no loss of precision
	unsigned int time = (distance * 1000) / velocity;

	drive_direct(velocity);

	ir_sleep(time);

	stop();
}

//drive in a straight line for the specified number of milliseconds at the specified speed
void drive_direct_duration(short velocity, unsigned int milliseconds){
	drive_direct(velocity);

	ir_sleep(milliseconds);

	stop();
}
*/

//rotate in place at the specified linear wheel velocity.  0 is clockwise.  1 is counter clockwise.
void turn_in_place(short velocity, int flag){
	
	if(flag){
		drive(velocity, velocity * -1);
	}
	else{
		drive(velocity * -1, velocity);
	}
}

//retutns the number of milliseconds it will take to rotate
//the specified number of degrees at the given velocity
unsigned int time_to_rotate_degrees(short velocity, int degrees){
	unsigned int velocity_diff = 2 * velocity;

	//the long-hand math.  this has all been compressed into one expression to avoid
	//loss of precision
	//unsigned int angular_velocity = velocity_diff / 258;	
	//int radians = (degrees * M_PI) / 180;
	//unsigned int time = radians / angular_velocity;
	
	//* 1000 == convert directly to milliseconds
	unsigned int time = (degrees * M_PI * WHEEL_BASE * 1000) / (180 * velocity_diff);

	return time;
}

/*
//rotate in place at the specified linear velocity through the specified number of degrees. 0=CW 1=CCW
void turn_in_place_degrees(short velocity, int flag, int degrees){
	unsigned int velocity_diff = 2 * velocity;

	//the long-hand math.  this has all been compressed into one expression to avoid
	//loss of precision
	//unsigned int angular_velocity = velocity_diff / 258;	
	//int radians = (degrees * M_PI) / 180;
	//unsigned int time = radians / angular_velocity;
	
	//* 1000 == convert directly to milliseconds
	unsigned int time = (degrees * M_PI * WHEEL_BASE * 1000) / (180 * velocity_diff);

	turn_in_place(velocity, flag);

	ir_sleep(time);

	stop();
}

//rotate in place for the specified number of milliseconds.
//i'm seriously considering not commenting the rest of this.
void turn_in_place_duration(short velocity, int flag, unsigned int milliseconds){
	turn_in_place(velocity, flag);

	ir_sleep(milliseconds);
	
	stop();
}
*/

//but i'm a tool...
//rotate about a given point (the radius).  radius > 0 is a CCW rotation.  radius < 0 is CW rotation.
//radius = 0 will likely cause the program to barf (but no guarantees since i haven't written the code yet).
//this is because there is no difference between +/-0
void turn_about(short velocity, short radius){
	ir_send_byte(CmdDrive);
	ir_send_byte(get_high_byte(velocity));
	ir_send_byte(get_low_byte(velocity));
	ir_send_byte(get_high_byte(radius));
	ir_send_byte(get_low_byte(radius));
}

//returns the number of milliseconds it will take to turn the specified
//number of degrees at the given velocity and raidus
unsigned int time_to_turn_about_degrees(short velocity, short radius, int degrees){
	//distance = (2 * radius * M_PI * degrees) / 360;
	//unsigned int time = distance / velocity;

	//implicit conversion to milliseconds
	unsigned int time = ((2 * radius * M_PI * degrees * 1000) / 360) / velocity;
	
	return time;
}

/*
//ditto, but for the specified number of degrees.
void turn_about_degrees(short velocity, short radius, int degrees){
	//distance = (radius * radius * M_PI * degrees) / 360;
	//unsigned int time = distance / velocity;

	//implicit conversion to milliseconds
	unsigned int time = (radius * radius * M_PI * degrees * 1000) / (360 * velocity);

	turn_about(velocity, radius);

	ir_sleep(time);

	stop();
}

int turn_about_degrees(short velocity, short radius, int degrees){
	//distance = (radius * radius * M_PI * degrees) / 360;
	//unsigned int time = distance / velocity;

	//implicit conversion to milliseconds
	unsigned int time = (radius * radius * M_PI * degrees * 1000) / (360 * velocity);

	turn_about(velocity, radius);
	
	return time;
}

//ditto, specified time.
void turn_about_duration(short velocity, short radius, unsigned int milliseconds){
	turn_about(velocity, radius);
	
	ir_sleep(milliseconds);
	
	stop();
}
*/


void leds(bool play,bool advance,unsigned short int color,unsigned short int intensity) {
	ir_send_byte(CmdLeds);
	ir_send_byte( ((char)play << 1)|((char)advance << 3) ); // play && advance 
	ir_send_byte(color <= 255 ? color : 255);
	ir_send_byte(intensity <= 255 ? intensity : 255);

}
